notebook.community

Edit and run

mobrob - A4

Implementieren Sie eine Beacon-Navigation unter Verwendung des Kalman-Filter; benutzen Sie hierfür ebenfalls den differentiellen Roboter. Es wird ein "Kompass" benötigt. Der Kompass liefert einen Winkel relativ zur X-Achse des Welt-KS, die Messung ist verrauscht. Desweiteren muss ein Bildsensor implementiert werden, der Markierungen (Beacons) aus dem Bild detektiert. Die Beacons sind an bekannten Orten im Raum angebracht ($X_{Li}$, $Y_{Li}$). Gemessen wird jeweils der Winkel (Azimuth) relativ zum Rob.-KS für ein zufällig ausgewähltes Beacon, die Winkelmessung ist ebenfalls verrauscht! Da die Identität der Beacons nicht ermittelt werden kann, ist die Verwendung eines "innovation gates" notwendig. Sollte ein Match nicht eindeutig möglich sein, wird die Messung verworfen.

Ablauf

  • Der Roboter bewegt sich, aktualisiert Positionsschätzung und deren Kovarianz anhand des Odometriemodells ($X_{k,k−1}$).
  • Gemessen wird jeweils der Winkel zu einem zufällig ausgewählten Beacon. Der Kalman-Filter aktualisiert die Schätzung ($X_{k,k}$,$C_{k,k}$).
  • Basisbreite (Abstand der Räder): $b$
  • Geschwindigkeiten der Räder: $\mathbf{v} = \left( v_L , v_R\right)^{\text{T}}$
  • Zustand (Pose im Weltkoordinatensystem): $\mathbf{X}_k = (x_k , y_k , \theta_k )^\text{T}$
  • Zustandsschätzung: $\mathbf{X}_{k,k} \overset{\text{Korrektur(Kalman)}}{\longleftarrow} \mathbf{X}_{k,k-1} \overset{\text{Vorhersage(Odom.-Modell)}}{\longleftarrow} \mathbf{X}_{k-1,k-1}$
  • Kovarianz der Positionsschätzung: $\mathbf{C}_{\text{P},k,k}$

Beacon-Peilung

Messgleichung: Ausgewählt wird Beacon $i$ aus der Karte $\alpha_k^i=\arctan\left({\frac{Y_{Li}-Y_k}{X_{Li}-X_k}}\right)-\theta_k - v_k^\text{P} = h^{\text{P}i}(\mathbf{X}_k,v_k^\text{P})$ mit $v_k^\text{P} \sim \mathcal{N}(0,\sigma_\text{P}^2)$

Kompass

Messgleichung: $\beta_k = \theta_k + v_k^\text{K} = h^\text{K} (\theta_k , v_k^\text{K})$ mit $v_k^\text{K} \sim \mathcal{N}(0,\sigma_\text{K}^2)$

Lösung

Dies sind die Lösungsschritte der Lokalisation anhand des Kalman-Filters mit Innovation-Gate.

Odometriegleichung

Verfahren nach Euler-Colaz für einen Roboter mit differentialer Lenkung:

\begin{align} \mathbf{X}_{k+1} = \begin{pmatrix} X_k\\ Y_k\\ \theta_k \end{pmatrix} + \begin{pmatrix} \frac{s_{Lk} + s_{Rk} }{2} \cos \left( \theta_k + \frac{s_{Rk} -s_{Lk}}{2b}\right)\\ \frac{s_{Lk} + s_{Rk} }{2} \sin \left( \theta_k + \frac{s_{Rk} -s_{Lk}}{2b}\right)\\ \frac{s_{Rk} - s_{Lk} }{b} \end{pmatrix} \text{ mit } s_{Lk}=v_{Lk}\Delta t \text{ und } s_{Rk}=v_{Rk}\Delta t \end{align}

Fehlerfortpflanzung der Odometrie:

\begin{align} \mathbf{C}_{\text{P},k+1,k} = \mathbf{F}_{\text{P},k+1} \mathbf{C}_{\text{P},k,k} \mathbf{F}_{\text{P},k+1}^{\text{T}} + \underbrace{\mathbf{F}_{\text{S},k+1} \mathbf{C}_{\text{S},k+1} \mathbf{F}_{\text{S},k+1}^{\text{T}}}_{\mathbf{Q}_{k+1}} \end{align}

Notiz:

$\mathbf{F}_{\text{P},k+1}$ ist die Jacobi-Matrix nach der Position $\mathbf{X}$ im Arbeitspunkt $k+1$ mit $\mathbf{F}_{\text{P},k+1} = \frac{\partial\mathbf{X}_{k+1}}{\partial\mathbf{X}}$

$\mathbf{F}_{\text{S},k+1}$ ist die Jacobi-Matrix nach der Geschwindigkeit $\mathbf{v}$ im Arbeitspunkt $k+1$ mit $\mathbf{F}_{\text{S},k+1} = \frac{\partial\mathbf{X}_{k+1}}{\partial\mathbf{v}}$

$\mathbf{C}_{\text{S},k+1}$ ist die Unsicherheit der zurückgelegten Wegstrecke, welche mit der Geschwindigkeit $\mathbf{v}$ skaliert (Die Konstanten $(k_1,k_2)$ müssen messtechnisch ermittelt werden) mit $\mathbf{C}_{\text{S},k+1}=\text{diag}\left(k_1 \left| s_{Lk+1}\right|, k_2 \left| s_{Rk+1}\right| \right)$

$\mathbf{Q}_{k+1}$ wird als die verkürzte Schreibweise für die Unsicherheit der Wegänderung verwendet, mit $\mathbf{Q}_{k+1} = \mathbf{F}_{\text{S},k+1} \mathbf{C}_{\text{S},k,k} \mathbf{F}_{\text{S},k+1}^{\text{T}}$

Sensordaten Einlesen

Diese Messgleichungen dienen der Simulation der Messungen zum Zeitpunkt $k+1$. Die Werte, die mit einem $'$ versehen sind, sind demnach ground truth Werte vom Simulator.

Beacon Messung

Sensormesswert wird von einem zufälligen Beacon $i$ unter dem Winkel $\alpha^i_{k+1}$ gemessen eingelesen:

\begin{align} &\tilde{z}^\text{P}_{k+1} = \alpha^i_{k+1} = \arctan\left({\frac{Y'_{Li}-Y'_{k+1}}{X'_{Li}-X'_{k+1}}}\right)-\theta'_{k+1} - v_{k+1}^\text{P} = h^{\text{P}i}(\mathbf{X}'_{k+1},v_{k+1}^\text{P}) \end{align}

mit $v_{k+1}^\text{P} \sim \mathcal{N}(0,\sigma_\text{P}^2)$.

und $i \sim$ Menge aller Beacons.

Kompass Messung

Die Kompassmessung ist eine verrauschte Messung der ground truth Daten.

\begin{align} &\tilde{z}^\text{K}_{k+1} = \beta_{k+1} = \theta'_{k+1} + v_{k+1}^\text{K} = h^{\text{K}i}(\mathbf{X}'_{k+1},v_{x+1}^\text{K}) \end{align}

mit $v_{k+1}^\text{K} \sim \mathcal{N}(0,\sigma_\text{K}^2)$

Berechnung einer Messvorhersage für alle Beacons

Hier müssen nun die Hypothesen $\hat{z}_{i,k+1}$ berechnet werden, unter welchem Winkel der Roboter an der Position $\mathbf{X}_{k+1}$ das Beacon $i$ messen kann.

\begin{align} \hat{z}_{i,k+1} = \arctan\left({\frac{Y_{Li}-Y_{k+1}}{X_{Li}-X_{k+1}}}\right)-\theta_k = h^{i}(\mathbf{X}_{k+1},0)\\ \end{align}

Kalman-Gain

Der Kalman-Gain wird anhand eines Innovation-Gates berechnet.

Berechnung der Jakobi-Matrix der Messgleichungen:

\begin{align} \mathbf{H}^i_{k+1} = \left. \frac{\partial (h^{\text{P}i}, h^{\text{K}i})^{\text{T}}}{ \partial \mathbf{X} }\right|_{\mathbf{X}_{k+1}} = \left( \begin{matrix} \frac{ Y_{Li}-Y_{k+1} }{(X_{Li}-X_{k+1})^2 + (Y_{Li}-Y_{k+1})^2} & \frac{ X_{Li}-X_{k+1} }{(X_{Li}-X_{k+1})^2 + (Y_{Li}-Y_{k+1})^2} & -1\\ 0 & 0 & 1 \end{matrix} \right) \end{align}
\begin{align} \mathbf{V}_{k+1} = \left. \frac{\partial (h^{\text{P}i}, h^{\text{K}i})^{\text{T}}}{ \partial \left( v^{\text{P}}, v^{\text{K}} \right)^{\text{T}} }\right|_{\mathbf{X}_{k+1}} = \left( \begin{matrix} -1 & 0\\ 0 & 1 \end{matrix} \right) \end{align}

Berechnung der Fehlerfortpflanzung der Messgleichung

Angabe der Unsicherheitsmatrix $\mathbf{N}_{k+1}$, welche vom System vorgegeben ist:

\begin{align} \mathbf{N}_{k+1} = \mathbf{N} = \left( \begin{matrix} \sigma_{\text{P}}^2 & 0\\ 0 & \sigma_{\text{K}}^2 \end{matrix} \right) = \text{const.} \forall k \end{align}

Fehlerfortpflanzung:

\begin{align} \mathbf{R}_{k+1} = \mathbf{V}_{k+1} \mathbf{N} \mathbf{V}_{k+1}^{\text{T}} = \left( \begin{matrix} \sigma_{\text{P}}^2 & 0\\ 0 & \sigma_{\text{K}}^2 \end{matrix} \right) = \text{const.} \forall k \end{align}

Notiz:

Die Unsicherheiten des Rauschens gehen additiv in die Messterme ein. Somit bleibt auch das Rauschen zwischen dem Kompass und der Beacon-Peilung zum einen immer unkorreliert, und zum anderen konstant über die Zeit.

Innovation Gate

Definition der Innovation: \begin{align} \nu{i,k+1} = \left( \begin{matrix} \nu^{\text{P}}{i,k+1}\ \nu^{\text{K}}_{i,k+1} \end{matrix}

\right)
=
\left(
\begin{matrix}        
    \tilde{z}^{\text{P}}_{k+1} - \hat{z}_{i,k+1}\\
    \tilde{z}^{\text{K}}_{k+1} - \theta_{k+1}
\end{matrix}
\right)

\end{align}

Berechnung der Kovarianz der Innovation für das Beacon $i$:

\begin{align} \mathbf{C}_{\nu_{i,k+1}} = \mathbf{H}^i_{k+1} \mathbf{C}_{\text{P},k+1,k} \mathbf{H}^{i\text{T}}_{k+1} + \mathbf{R}_{k+1} \end{align}

Vergleich der Innovation einer Messgleichung vom Beacon $i$ zu einem festen, definierten Gate-Wert $g$:

\begin{align} \nu_{i,k+1}^{\text{T}} \mathbf{C}_{\nu_{i,k+1}}^{-1} \nu_{i,k+1} \le g^2 \end{align}

Falls die Gleichung für einen Beacon $i$ wahr ist, so wird dieser der Menge $\mathcal{M}_{k+1}$ hinzugefügt

Kalman-Gain

Falls anhand des Innovation-Gates ein einziger Match $\mathcal{M}_{k+1}$ gefunden wurde, wird der Kalman-Gain berechnet. Wird kein oder mehrer Matches $\mathcal{M}_{k+1}$ gefunden, wird der Kalman-Gain verworfen.

\begin{align} \mathbf{K}_{k+1} = \left\lbrace \begin{matrix} \mathbf{C}_{\text{P},k+1,k} \mathbf{H}^{i\text{T}}_{k+1} \mathbf{C}_{\nu_{i,k+1}}^{-1} & \text{falls }|\mathcal{M}_{k+1}| = 1\\ \mathbf{0} & \text{falls }|\mathcal{M}_{k+1}| \neq 1 \end{matrix} \right. \end{align}

Aktualisierung der Position und Unsicherheit

Falls das Matching eines Beacons $i$ anhand des Innovation-Gates erfolgreich war, so wird die Sensormessung beim Update berücksichtigt. Falls es jedoch nicht erfolgreich war, so wird die Messung verworfen und lediglich die Odometrie verwendet.

Update: Position

\begin{align} \mathbf{X}_{k+1,k+1} = \mathbf{X}_{k+1,k} + \mathbf{K}_{k+1} \nu_{i,k+1} \end{align}

Update: Kovarianz

\begin{align} \mathbf{C}_{\text{P},k+1,k+1} = \mathbf{C}_{\text{P},k+1,k} - \mathbf{K}_{k+1} \mathbf{C}_{\nu_{i,k+1}} \mathbf{K}_{k+1}^{\text{T}} \end{align}

Iteration

Setze nun $k+1 \rightarrow k$ und beginne wieder im Odometrieschritt.

Content source: tik0/mobrob

Similar notebooks:

notebook.community | gallery | about